/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package test.ashen;

import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D64f;
import com.googlecode.javacv.cpp.opencv_video.CvKalman;
import com.googlecode.javacv.cpp.opencv_calib3d;
import java.awt.event.KeyEvent;
import com.googlecode.javacv.cpp.opencv_calib3d.*;
import com.googlecode.javacv.FrameGrabber.Exception;
import java.util.logging.Level;
import java.util.logging.Logger;
import static com.googlecode.javacv.cpp.opencv_core.*;
import com.googlecode.javacv.*;
import com.googlecode.javacv.cpp.opencv_calib3d.*;
import com.googlecode.javacv.cpp.opencv_calib3d.CvStereoBMState;
import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import com.googlecode.javacv.cpp.opencv_video;
import static com.googlecode.javacv.cpp.opencv_calib3d.cvCreateStereoBMState;
import static com.googlecode.javacv.cpp.opencv_calib3d.cvFindStereoCorrespondenceBM;
import static com.googlecode.javacv.cpp.opencv_calib3d.CV_STEREO_BM_BASIC;
import static com.googlecode.javacv.cpp.opencv_imgproc.*;

/**
 *
 * @author Home
 */
public class KalmanFil {

    public  static CvKalman filter;
    CvMat state;

    public CvPoint filtering(CvPoint measurement) {

        KalmanFil.filter.state_pre().put(0, 0, measurement.x());
        KalmanFil.filter.state_pre().put(1, 0, measurement.y());
        KalmanFil.filter.state_pre().put(2, 0, 0);
        KalmanFil.filter.state_pre().put(3, 0, 0);

        cvRange(KalmanFil.filter.state_post(), 0, 0.1);
        System.out.println(KalmanFil.filter.measurement_matrix().get(1, 1));

        System.out.println("state row="+KalmanFil.filter.state_post().rows());
        System.out.println("state col="+KalmanFil.filter.state_post().cols());

        CvMat prediction=CvMat.create(1,4);
        prediction=opencv_video.cvKalmanPredict(KalmanFil.filter,null);


        CvPoint2D64f predictedpoint = cvPoint2D64f(prediction.get(0, 0), prediction.get(1,0 ));

        CvMat meas=CvMat.create(2, 1);
        meas.put(0, 0, measurement.x());
        meas.put(1, 0, measurement.y());
        CvMat meas1=CvMat.create(2, 1);
        cvMatMul(KalmanFil.filter.measurement_matrix(), state, meas1);
        cvAdd(meas1, meas, meas, null);

        CvMat est=CvMat.create(2, 1);
        est=opencv_video.cvKalmanCorrect(KalmanFil.filter, meas);

        CvPoint estpoint=cvPoint((int)est.get(0, 0),(int) est.get(1, 0));

        CvMat pronoise=CvMat.create(4, 1);
        cvRange(pronoise, 0, Math.sqrt(KalmanFil.filter.process_noise_cov().get(0, 0)));

        cvMatMul( KalmanFil.filter.transition_matrix(),state, state);
        cvAdd(state, pronoise, state, null);


        return estpoint;
    }

    public KalmanFil() {
        //System.out.println("filter build");
        state=CvMat.create(4, 1);
        KalmanFil.filter = CvKalman.create(4, 2, 0);
        CvMat gh = CvMat.create(4, 4);
        gh.put(0, 0, 1);
        gh.put(0, 1, 0);
        gh.put(0, 2, 1);
        gh.put(0, 3, 0);

        gh.put(1, 0, 0);
        gh.put(1, 1, 1);
        gh.put(1, 2, 0);
        gh.put(1, 3, 1);

        gh.put(2, 0, 0);
        gh.put(2, 1, 0);
        gh.put(2, 2, 1);
        gh.put(2, 3, 0);

        gh.put(3, 0, 0);
        gh.put(3, 1, 0);
        gh.put(3, 2, 0);
        gh.put(3, 3, 1);

        System.out.println("tr col="+KalmanFil.filter.transition_matrix().cols());
        System.out.println("tr rows="+KalmanFil.filter.transition_matrix().rows());


        KalmanFil.filter.transition_matrix(gh);

        cvSetIdentity(KalmanFil.filter.measurement_matrix());
        System.out.println("meas col="+KalmanFil.filter.measurement_matrix().cols());
        System.out.println("meas row="+KalmanFil.filter.measurement_matrix().rows());
        System.out.println(filter.measurement_matrix().get(1, 2));


        CvScalar cvs1 = cvScalarAll(1e-4);
        cvSetIdentity(KalmanFil.filter.process_noise_cov(), cvs1);
        System.out.println("pro no cov ro="+KalmanFil.filter.process_noise_cov().rows());
        System.out.println("pro no cov co="+KalmanFil.filter.process_noise_cov().cols());
//        System.out.println(KalmanFil.filter.process_noise_cov().get(0, 0));
//        System.out.println(KalmanFil.filter.process_noise_cov().get(0, 1));

        CvScalar cvs2 = cvScalarAll(1e-1);
        cvSetIdentity(KalmanFil.filter.measurement_noise_cov(), cvs2);

        CvScalar cvs3 = cvScalarAll(0.1);
        cvSetIdentity(KalmanFil.filter.error_cov_post(), cvs3);


        System.out.println("filter build");

    }




    public static void main(String[] args) {
//        KalmanFil kf = new KalmanFil();
//        cvRange(kf.filter.state_post(), 0, 0.1);
//        System.out.println(kf.filter.state_post().get(1, 2));
        double r=90.5;
        System.out.println((int)(r));

        CvMat a = CvMat.create(1, 2);
        a.put(0, 0, 2);
        a.put(0, 1, 1);

       

        CvMat b = CvMat.create( 2,2,1111638021);
        b.put(0,0,6);
        b.put(1,0,3);
//
//          CvMat c = CvMat.create(1, 1);
//          cvMatMul(a, b, c);
//          System.out.println(c.get(0, 0));
//
//
//        cvAdd(a, b, c, null);
//        System.out.println(c.get(0, 0));

          //cvSetIdentity(b, 1);
          cvRange(b, 0, 10);
          System.out.println(b.get(0, 0));
          System.out.println(b.get(1, 1));
           System.out.println(b.get(1, 0));
//          cvConvertScale(b, b, 0, 1);
//          System.out.println(b.get(0, 0));
//          System.out.println(b.get(0, 1));
//          int i=158;
//          System.out.println(i);




    }
}
